#include "utility.hpp"
#include "lio_sam/msg/cloud_info.hpp"

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ndt.h>
#include <pcl/common/centroid.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/sac_model_registration.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <rclcpp/qos.hpp>

using namespace gtsam;

using symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
using symbol_shorthand::G; // GPS pose
using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)

/*
 * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
 */
struct PointXYZIRPYT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY; // preferred way of adding a XYZ+padding
    float roll;
    float pitch;
    float yaw;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignment

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRPYT,
                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, roll, roll)(float, pitch, pitch)(float, yaw, yaw)(double, time, time))

typedef PointXYZIRPYT PointTypePose;

class mapOptimization : public ParamServer
{

public:
    // gtsam
    NonlinearFactorGraph gtSAMgraph;
    Values initialEstimate;
    Values optimizedEstimate;
    ISAM2 *isam;
    Values isamCurrentEstimate;
    Eigen::MatrixXd poseCovariance;

    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudSurround;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMappedROS;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubKeyPoses;
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubPath;

    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubHistoryKeyFrames;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubIcpKeyFrames;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubRecentKeyFrames;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubRecentKeyFrame;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubCloudRegisteredRaw;

    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubDebugClusters_;

    rclcpp::Subscription<lio_sam::msg::CloudInfo>::SharedPtr subLaserCloudInfo;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subGPS;

    rclcpp::TimerBase::SharedPtr localize_timer_;
    rclcpp::TimerBase::SharedPtr map_publish_timer_;

    std::deque<nav_msgs::msg::Odometry> gpsQueue;
    lio_sam::msg::CloudInfo cloudInfo;

    vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
    vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
    vector<pcl::PointCloud<PointType>::Ptr> intensityCloudKeyFrames;

    pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;     // gc: can be used to illustrate the path of odometry // keep
    pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D; // gc: can be used to illustrate the path of odometry //keep
    // addded**********************************by gc
    std::mutex mtxWin;
    std::vector<PointType> win_cloudKeyPoses3D;
    std::vector<PointTypePose> win_cloudKeyPoses6D;

    std::vector<pcl::PointCloud<PointType>::Ptr> win_cornerCloudKeyFrames;
    std::vector<pcl::PointCloud<PointType>::Ptr> win_surfCloudKeyFrames;
    // added***********************************by gc

    pcl::PointCloud<PointType>::Ptr laserCloudCornerLast;   // corner feature set from odoOptimization
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLast;     // surf feature set from odoOptimization
    pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization
    pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS;   // downsampled surf featuer set from odoOptimization

    pcl::PointCloud<PointType>::Ptr laserCloudOri;
    pcl::PointCloud<PointType>::Ptr coeffSel;

    std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computation
    std::vector<PointType> coeffSelCornerVec;
    std::vector<bool> laserCloudOriCornerFlag;
    std::vector<PointType> laserCloudOriSurfVec; // surf point holder for parallel computation
    std::vector<PointType> coeffSelSurfVec;
    std::vector<bool> laserCloudOriSurfFlag;

    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
    pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
    pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;

    pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;
    pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;

    // pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
    // pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;

    pcl::PointCloud<PointType>::Ptr latestKeyFrameCloud;
    pcl::PointCloud<PointType>::Ptr nearHistoryKeyFrameCloud;

    pcl::VoxelGrid<PointType> downSizeFilterCorner;
    pcl::VoxelGrid<PointType> downSizeFilterSurf;
    pcl::VoxelGrid<PointType> downSizeFilterICP;
    pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization

    rclcpp::Time timeLaserInfoStamp;
    double timeLaserCloudInfoLast;

    float transformTobeMapped[6];

    std::mutex mtx;

    double timeLastProcessing = -1;

    bool isDegenerate = false;
    Eigen::Matrix<float, 6, 6> matP;

    int winSize = 30;
    int laserCloudCornerFromMapDSNum = 0;
    int laserCloudSurfFromMapDSNum = 0;
    int laserCloudCornerLastDSNum = 0;
    int laserCloudSurfLastDSNum = 0;

    bool aLoopIsClosed = false;
    int imuPreintegrationResetId = 0;

    nav_msgs::msg::Path globalPath;

    Eigen::Affine3f transPointAssociateToMap;

    /*************added by gc*****************/
    pcl::PointCloud<PointType>::Ptr cloudGlobalMap;
    pcl::PointCloud<PointType>::Ptr cloudGlobalMapDS;
    pcl::PointCloud<PointType>::Ptr cloudScanForInitialize;

    pcl::PointCloud<PointType>::Ptr pillarMap;
    pcl::KdTreeFLANN<PointType>::Ptr kdtreePillarMap;

    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subIniPoseFromRviz;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudInWorld;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubMapWorld;
    // rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr fortest_publasercloudINWorld;

    float transformInTheWorld[6]; // the pose in the world, i.e. the prebuilt map
    float tranformOdomToWorld[6];
    int globalLocaSkipFrames = 3;
    int frameNum = 1;
    std::unique_ptr<tf2_ros::TransformBroadcaster> br;
    std::mutex mtxtranformOdomToWorld;
    std::mutex mtx_general;
    bool globalLocalizeInitialized = false;

    std::optional<lio_sam::msg::CloudInfo> cloudInfoForInitialization;

    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subImu;

    enum InitializedFlag
    {
        NonInitialized,
        Initializing,
        Initialized
    };
    InitializedFlag initializedFlag;

    geometry_msgs::msg::PoseStamped poseOdomToMap;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pubOdomToMapPose;

    /*************added by gc******************/

    mapOptimization(const rclcpp::NodeOptions &options) : ParamServer("lio_sam_mapOptimization", options)
    {
        ISAM2Params parameters;
        parameters.relinearizeThreshold = 0.1;
        parameters.relinearizeSkip = 1;
        isam = new ISAM2(parameters);

        pubKeyPoses = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/mapping/trajectory", 1);
        pubLaserCloudSurround = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/mapping/map_global", 1);
        pubOdomAftMappedROS = create_publisher<nav_msgs::msg::Odometry>("lio_sam/mapping/odometry", 1);
        pubPath = create_publisher<nav_msgs::msg::Path>("lio_sam/mapping/path", 1);

        auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort();
        subLaserCloudInfo = create_subscription<lio_sam::msg::CloudInfo>(
            "lio_sam/feature/cloud_info", qos,
            std::bind(&mapOptimization::laserCloudInfoHandler, this, std::placeholders::_1));
        subGPS = create_subscription<nav_msgs::msg::Odometry>(gpsTopic, 200, std::bind(&mapOptimization::gpsHandler, this, std::placeholders::_1));

        // added ******************by gc
        subIniPoseFromRviz = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/initialpose", 8, std::bind(&mapOptimization::initialpose_callback, this, std::placeholders::_1));
        pubMapWorld = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/mapping/cloud_map_map", 1); //
        // fortest_publasercloudINWorld = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/mapping/laserclouinmapframe",1);
        pubLaserCloudInWorld = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/mapping/lasercloud_in_world", 1); // added
        pubOdomToMapPose = create_publisher<geometry_msgs::msg::PoseStamped>("lio_sam/mapping/pose_odomTo_map", 1);
        pubDebugClusters_ = create_publisher<sensor_msgs::msg::PointCloud2>("debug/clusters", 1);

        // subImu      = create_subscription<sensor_msgs::msg::Imu>  (imuTopic,  200, std::bind(&mapOptimization::imuHandler, this, std::placeholders::_1));
        // added ******************by gc

        pubHistoryKeyFrames = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/mapping/icp_loop_closure_history_cloud", 1);
        pubIcpKeyFrames = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1);

        pubRecentKeyFrames = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/mapping/map_local", 1);
        pubRecentKeyFrame = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/mapping/cloud_registered", 1);
        pubCloudRegisteredRaw = create_publisher<sensor_msgs::msg::PointCloud2>("lio_sam/mapping/cloud_registered_raw", 1);

        br = std::make_unique<tf2_ros::TransformBroadcaster>(this);

        downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
        downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
        downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
        downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization

        allocateMemory();
        map_publish_timer_ = this->create_wall_timer(std::chrono::seconds(2), [this]() -> void
                                                     {
            // 发布用于可视化的全局地图
            if (pubMapWorld->get_subscription_count() > 0 && cloudGlobalMapDS && !cloudGlobalMapDS->empty())
            {
                sensor_msgs::msg::PointCloud2 map_msg;
                pcl::toROSMsg(*cloudGlobalMapDS, map_msg);
                map_msg.header.stamp = this->get_clock()->now();
                map_msg.header.frame_id = "map";
                pubMapWorld->publish(map_msg);
            } });
        auto localize_period = std::chrono::seconds(1);
        localize_timer_ = this->create_wall_timer(
            localize_period,
            std::bind(&mapOptimization::globalLocalizeTimerCallback, this));
    }

    void allocateMemory()
    {
        cloudGlobalMap.reset(new pcl::PointCloud<PointType>());   // addded by gc
        cloudGlobalMapDS.reset(new pcl::PointCloud<PointType>()); // added
        cloudScanForInitialize.reset(new pcl::PointCloud<PointType>());
        resetLIO();
        // added by gc
        for (int i = 0; i < 6; ++i)
        {
            transformInTheWorld[i] = 0;
        }

        for (int i = 0; i < 6; ++i)
        {
            tranformOdomToWorld[i] = 0;
        }
        initializedFlag = NonInitialized;
        loadMaps();
    }
    void resetLIO()
    {
        RCLCPP_INFO(get_logger(), "\033[1;33mPerforming full LIO backend reset...\033[0m");

        // ====================== 核心修改：彻底重置GTSAM ======================
        // 1. 释放旧的ISAM2对象的内存
        if (isam)
        {
            delete isam;
            isam = nullptr;
        }

        // 2. 重新创建一个全新的、空的ISAM2对象
        ISAM2Params parameters;
        parameters.relinearizeThreshold = 0.1;
        parameters.relinearizeSkip = 1;
        isam = new ISAM2(parameters);

        // 3. 清空GTSAM的因子图和初始估计
        gtSAMgraph.resize(0);
        initialEstimate.clear();
        // =================================================================

        // --- 清空我们自己的容器 (这部分和您原来的代码应该是一样的) ---
        cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
        cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());

        cornerCloudKeyFrames.clear();
        surfCloudKeyFrames.clear();
        intensityCloudKeyFrames.clear();
        // 如果您有其他关键帧相关的容器，也在这里清空，例如：
        // groundCloudKeyFrames.clear();
        // pillarCloudKeyFrames.clear();

        // 清空滑动窗口相关的容器 (如果您使用了的话)
        win_cloudKeyPoses3D.clear();
        win_cloudKeyPoses6D.clear();
        win_cornerCloudKeyFrames.clear();
        win_surfCloudKeyFrames.clear();

        laserCloudCornerLast.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfLast.reset(new pcl::PointCloud<PointType>());
        laserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>());

        laserCloudOri.reset(new pcl::PointCloud<PointType>());
        coeffSel.reset(new pcl::PointCloud<PointType>());

        laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
        coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
        coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
        laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);

        std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
        std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);

        laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>());
        laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
        laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());

        kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
        kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());

        latestKeyFrameCloud.reset(new pcl::PointCloud<PointType>());
        nearHistoryKeyFrameCloud.reset(new pcl::PointCloud<PointType>());

        for (int i = 0; i < 6; ++i)
        {
            transformTobeMapped[i] = 0;
        }

        matP.setZero();

        // 清空全局路径，让它从新的起点重新绘制
        globalPath.poses.clear();
    }

    void laserCloudInfoHandler(const lio_sam::msg::CloudInfo::SharedPtr msgIn)
    {
        // extract time stamp
        // added
        cloudInfo = *msgIn;

        timeLaserInfoStamp = msgIn->header.stamp;
        timeLaserCloudInfoLast = stamp2Sec(msgIn->header.stamp);
        if (initializedFlag == NonInitialized || initializedFlag == Initializing)
        {
            // 捕获并存储第一帧完整的 cloud_info 消息
            std::lock_guard<std::mutex> lock(mtx_general);
            if (!cloudInfoForInitialization)
            {
                cloudInfoForInitialization = *msgIn;
                RCLCPP_INFO(get_logger(), "Captured first cloud info for initialization.");
            }
            // 在定位成功之前，不执行任何LIO建图
            return;
        }

        pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
        pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);

        /************************************added by gc*****************************/

        // if the sysytem is not initialized ffer the first scan for the system to initialize
        // the LIO system stsrt working only when the localization initializing is finished
        if (initializedFlag == NonInitialized || initializedFlag == Initializing)
        {

            if (cloudScanForInitialize->empty())
            {

                downsampleCurrentScan();
                *cloudScanForInitialize += *laserCloudCornerLastDS;
            }
            return;
        }

        frameNum++;

        /************************************added by gc*****************************/

        std::lock_guard<std::mutex> lock(mtx);

        if (timeLaserCloudInfoLast - timeLastProcessing >= mappingProcessInterval)
        { // gc:control the rate of mapping process

            timeLastProcessing = timeLaserCloudInfoLast;

            updateInitialGuess(); // gc: update initial value for states

            extractSurroundingKeyFrames(); // gc:

            downsampleCurrentScan(); // gc:down sample the current corner points and surface points

            scan2MapOptimization(); // gc: calculate the tranformtion using lidar measurement with the Imu preintegration as initial values
            // and then interpolate roll and pitch angle using IMU measurement and above measurement

            saveKeyFramesAndFactor(); // gc: save corner cloud and surface cloud of this scan, and add odom and GPS factors

            // correctPoses();

            publishOdometry();

            publishFrames();
        }
    }

    void gpsHandler(const nav_msgs::msg::Odometry::SharedPtr gpsMsg)
    {
        gpsQueue.push_back(*gpsMsg);
    }

    void pointAssociateToMap(PointType const *const pi, PointType *const po)
    {
        po->x = transPointAssociateToMap(0, 0) * pi->x + transPointAssociateToMap(0, 1) * pi->y + transPointAssociateToMap(0, 2) * pi->z + transPointAssociateToMap(0, 3);
        po->y = transPointAssociateToMap(1, 0) * pi->x + transPointAssociateToMap(1, 1) * pi->y + transPointAssociateToMap(1, 2) * pi->z + transPointAssociateToMap(1, 3);
        po->z = transPointAssociateToMap(2, 0) * pi->x + transPointAssociateToMap(2, 1) * pi->y + transPointAssociateToMap(2, 2) * pi->z + transPointAssociateToMap(2, 3);
        po->intensity = pi->intensity;
    }

    pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose *transformIn)
    {
        pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());

        PointType *pointFrom;

        int cloudSize = cloudIn->size();
        cloudOut->resize(cloudSize);

        Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);

        for (int i = 0; i < cloudSize; ++i)
        {

            pointFrom = &cloudIn->points[i];
            cloudOut->points[i].x = transCur(0, 0) * pointFrom->x + transCur(0, 1) * pointFrom->y + transCur(0, 2) * pointFrom->z + transCur(0, 3);
            cloudOut->points[i].y = transCur(1, 0) * pointFrom->x + transCur(1, 1) * pointFrom->y + transCur(1, 2) * pointFrom->z + transCur(1, 3);
            cloudOut->points[i].z = transCur(2, 0) * pointFrom->x + transCur(2, 1) * pointFrom->y + transCur(2, 2) * pointFrom->z + transCur(2, 3);
            cloudOut->points[i].intensity = pointFrom->intensity;
        }
        return cloudOut;
    }

    gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint)
    {
        return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
                            gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
    }

    gtsam::Pose3 trans2gtsamPose(float transformIn[])
    {
        return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]),
                            gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
    }

    Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint)
    {
        return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw);
    }

    Eigen::Affine3f trans2Affine3f(float transformIn[])
    {
        return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]);
    }

    PointTypePose trans2PointTypePose(float transformIn[])
    {
        PointTypePose thisPose6D;
        thisPose6D.x = transformIn[3];
        thisPose6D.y = transformIn[4];
        thisPose6D.z = transformIn[5];
        thisPose6D.roll = transformIn[0];
        thisPose6D.pitch = transformIn[1];
        thisPose6D.yaw = transformIn[2];
        return thisPose6D;
    }

    void updateInitialGuess()
    {
        // 这个函数的作用是为当前帧提供一个位姿初始猜测
        // 我们将它恢复到与您能正常工作的SLAM版本相似的、正确的“增量式”更新逻辑

        static Eigen::Affine3f lastImuTransformation;
        // 初始化：如果是第一帧，直接使用IMU的姿态作为初始姿态
        if (cloudKeyPoses3D->points.empty())
        {
            transformTobeMapped[0] = cloudInfo.imu_roll_init;
            transformTobeMapped[1] = cloudInfo.imu_pitch_init;
            transformTobeMapped[2] = cloudInfo.imu_yaw_init;

            // 根据参数决定是否使用IMU的偏航角作为初始值
            if (!useImuHeadingInitialization)
                transformTobeMapped[2] = 0;

            // 保存这一帧的IMU姿态，用于计算下一帧的增量
            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init);
            return;
        }

        // 增量式更新：使用IMU预积分的增量来预测当前帧位姿
        static bool lastImuPreTransAvailable = false;
        static Eigen::Affine3f lastImuPreTransformation;

        // 您的重定位版本中，预积分信息直接存在于cloudInfo中
        if (cloudInfo.odom_available == true)
        {
            // 获取当前帧的IMU预积分里程计位姿 (这是一个带有漂移的绝对位姿)
            Eigen::Affine3f transBack = pcl::getTransformation(
                cloudInfo.initial_guess_x, cloudInfo.initial_guess_y, cloudInfo.initial_guess_z,
                cloudInfo.initial_guess_roll, cloudInfo.initial_guess_pitch, cloudInfo.initial_guess_yaw);

            if (lastImuPreTransAvailable == false)
            {
                // 如果是第一次收到预积分里程计，只保存状态，不计算增量
                lastImuPreTransformation = transBack;
                lastImuPreTransAvailable = true;
            }
            else
            {
                // 1. 计算从上一帧到当前帧的、短时间内非常准确的“运动增量”
                Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;

                // 2. 获取上一帧经过后端优化后的“精确位姿”
                Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);

                // 3. 将运动增量应用到上一帧的精确位姿上，得到当前帧的高质量初始猜测
                Eigen::Affine3f transFinal = transTobe * transIncre;

                // 4. 更新 transformTobeMapped 作为初始值
                pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
                                                  transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

                // 5. 更新“上一帧”的预积分状态，为下一次计算做准备
                lastImuPreTransformation = transBack;
            }
        }
        // 备用方案：如果没有IMU预积分信息，则使用纯IMU的旋转增量做预测
        else if (cloudInfo.imu_available == true)
        {
            Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init);
            Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;

            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            Eigen::Affine3f transFinal = transTobe * transIncre;
            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
                                              transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
        }

        // 无论如何，都保存当前帧的IMU姿态，给下一帧使用
        lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init);
    }

    void extractForLoopClosure()
    {
        // change-1
        /**************gc added**************/
        //{
        // in this place the maximum of numPoses is winSize
        pcl::PointCloud<PointType>::Ptr cloudToExtract(new pcl::PointCloud<PointType>());
        int numPoses = win_cloudKeyPoses3D.size();
        for (int i = numPoses - 1; i >= 0; --i)
        {
            cloudToExtract->push_back(win_cloudKeyPoses3D[i]);
        }
        extractCloud(cloudToExtract);
        // }
        /**************gc added**************/

        //        {
        //            pcl::PointCloud<PointType>::Ptr cloudToExtract(new pcl::PointCloud<PointType>());
        //            int numPoses = cloudKeyPoses3D->size();
        //            for (int i = numPoses-1; i >= 0; --i)
        //            {
        //                if ((int)cloudToExtract->size() <= surroundingKeyframeSize)
        //                    cloudToExtract->push_back(cloudKeyPoses3D->points[i]);
        //                else
        //                    break;
        //            }
        //
        //            extractCloud(cloudToExtract);
        //        }
    }
    // gc:search nearby key poses and downsample them and then extract the local map points
    //    void extractNearby()
    //    {
    //
    //    }
    // gc: extract the nearby Map points
    void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
    {
        // change-2

        /**************gc added**************/
        // std::cout << "cloudToExtract size: " << cloudToExtract->size() << std::endl;

        std::vector<pcl::PointCloud<PointType>> laserCloudCornerSurroundingVec;
        std::vector<pcl::PointCloud<PointType>> laserCloudSurfSurroundingVec;

        laserCloudCornerSurroundingVec.resize(cloudToExtract->size());
        laserCloudSurfSurroundingVec.resize(cloudToExtract->size());

        // extract surrounding map
#pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < (int)cloudToExtract->size(); ++i)
        {
            PointTypePose thisPose6D;
            thisPose6D = win_cloudKeyPoses6D[i];
            laserCloudCornerSurroundingVec[i] = *transformPointCloud(win_cornerCloudKeyFrames[i], &thisPose6D);
            laserCloudSurfSurroundingVec[i] = *transformPointCloud(win_surfCloudKeyFrames[i], &thisPose6D);
        }

        // fuse the map
        laserCloudCornerFromMap->clear();
        laserCloudSurfFromMap->clear();
        for (int i = 0; i < (int)cloudToExtract->size(); ++i)
        {
            *laserCloudCornerFromMap += laserCloudCornerSurroundingVec[i];
            *laserCloudSurfFromMap += laserCloudSurfSurroundingVec[i];
        }

        // Downsample the surrounding corner key frames (or map)
        downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
        downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
        laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
        // std::cout << "the size of laserCloudCornerFromMapDS: " << laserCloudCornerFromMapDSNum << std::endl;
        //  Downsample the surrounding surf key frames (or map)
        downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
        downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
        laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
    }

    void extractSurroundingKeyFrames()
    {
        if (cloudKeyPoses3D->points.empty() == true)
            return;

        // if (loopClosureEnableFlag == true)//gc:TODO: a little weired: Loop closure should search the whole map while
        //{
        // std::cout << "the size of cloudKeyPoses3D: " << cloudKeyPoses3D->points.size() << std::endl;
        extractForLoopClosure(); // gc: the name is misleading
                                 //} else {
                                 // extractNearby();
                                 //}
    }

    void downsampleCurrentScan()
    {
        // Downsample cloud from current scan
        laserCloudCornerLastDS->clear();
        downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
        downSizeFilterCorner.filter(*laserCloudCornerLastDS);
        laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();

        laserCloudSurfLastDS->clear();
        downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
        downSizeFilterSurf.filter(*laserCloudSurfLastDS);
        laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
    }

    void updatePointAssociateToMap()
    {
        transPointAssociateToMap = trans2Affine3f(transformTobeMapped);
    }

    void cornerOptimization()
    {
        updatePointAssociateToMap();

#pragma omp parallel for num_threads(numberOfCores)
        // gc: for every corner point
        for (int i = 0; i < laserCloudCornerLastDSNum; i++)
        {
            PointType pointOri, pointSel, coeff;
            std::vector<int> pointSearchInd;
            std::vector<float> pointSearchSqDis;

            pointOri = laserCloudCornerLastDS->points[i];
            // gc: calculate its location in the map using the prediction pose
            pointAssociateToMap(&pointOri, &pointSel);
            kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

            cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
            cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
            cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));

            if (pointSearchSqDis[4] < 1.0)
            {
                float cx = 0, cy = 0, cz = 0;
                for (int j = 0; j < 5; j++)
                {
                    cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
                    cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
                    cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
                }
                // gc: the average coordinate of the most nearest points
                cx /= 5;
                cy /= 5;
                cz /= 5;

                float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
                for (int j = 0; j < 5; j++)
                {
                    float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
                    float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
                    float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;

                    a11 += ax * ax;
                    a12 += ax * ay;
                    a13 += ax * az;
                    a22 += ay * ay;
                    a23 += ay * az;
                    a33 += az * az;
                }
                a11 /= 5;
                a12 /= 5;
                a13 /= 5;
                a22 /= 5;
                a23 /= 5;
                a33 /= 5;

                matA1.at<float>(0, 0) = a11;
                matA1.at<float>(0, 1) = a12;
                matA1.at<float>(0, 2) = a13;
                matA1.at<float>(1, 0) = a12;
                matA1.at<float>(1, 1) = a22;
                matA1.at<float>(1, 2) = a23;
                matA1.at<float>(2, 0) = a13;
                matA1.at<float>(2, 1) = a23;
                matA1.at<float>(2, 2) = a33;

                cv::eigen(matA1, matD1, matV1);

                if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1))
                {

                    float x0 = pointSel.x;
                    float y0 = pointSel.y;
                    float z0 = pointSel.z;
                    float x1 = cx + 0.1 * matV1.at<float>(0, 0);
                    float y1 = cy + 0.1 * matV1.at<float>(0, 1);
                    float z1 = cz + 0.1 * matV1.at<float>(0, 2);
                    float x2 = cx - 0.1 * matV1.at<float>(0, 0);
                    float y2 = cy - 0.1 * matV1.at<float>(0, 1);
                    float z2 = cz - 0.1 * matV1.at<float>(0, 2);

                    float a012 = sqrt(((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) + ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) + ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)));

                    float l12 = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2));

                    float la = ((y1 - y2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) + (z1 - z2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1))) / a012 / l12;

                    float lb = -((x1 - x2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) - (z1 - z2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / a012 / l12;

                    float lc = -((x1 - x2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) + (y1 - y2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / a012 / l12;

                    float ld2 = a012 / l12;

                    float s = 1 - 0.9 * fabs(ld2);

                    coeff.x = s * la;
                    coeff.y = s * lb;
                    coeff.z = s * lc;
                    coeff.intensity = s * ld2;

                    if (s > 0.1)
                    {
                        laserCloudOriCornerVec[i] = pointOri;
                        coeffSelCornerVec[i] = coeff;
                        laserCloudOriCornerFlag[i] = true;
                    }
                }
            }
        }
    }

    void surfOptimization()
    {
        updatePointAssociateToMap();

#pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < laserCloudSurfLastDSNum; i++)
        {
            PointType pointOri, pointSel, coeff;
            std::vector<int> pointSearchInd;
            std::vector<float> pointSearchSqDis;

            pointOri = laserCloudSurfLastDS->points[i];
            pointAssociateToMap(&pointOri, &pointSel);
            kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

            Eigen::Matrix<float, 5, 3> matA0;
            Eigen::Matrix<float, 5, 1> matB0;
            Eigen::Vector3f matX0;

            matA0.setZero();
            matB0.fill(-1);
            matX0.setZero();

            if (pointSearchSqDis[4] < 1.0)
            {
                for (int j = 0; j < 5; j++)
                {
                    matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                    matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                    matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
                }

                matX0 = matA0.colPivHouseholderQr().solve(matB0);

                float pa = matX0(0, 0);
                float pb = matX0(1, 0);
                float pc = matX0(2, 0);
                float pd = 1;

                float ps = sqrt(pa * pa + pb * pb + pc * pc);
                pa /= ps;
                pb /= ps;
                pc /= ps;
                pd /= ps;

                bool planeValid = true;
                for (int j = 0; j < 5; j++)
                {
                    if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
                             pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                             pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2)
                    {
                        planeValid = false;
                        break;
                    }
                }

                if (planeValid)
                {
                    float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

                    float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x + pointSel.y * pointSel.y + pointSel.z * pointSel.z));

                    coeff.x = s * pa;
                    coeff.y = s * pb;
                    coeff.z = s * pc;
                    coeff.intensity = s * pd2;

                    if (s > 0.1)
                    {
                        laserCloudOriSurfVec[i] = pointOri;
                        coeffSelSurfVec[i] = coeff;
                        laserCloudOriSurfFlag[i] = true;
                    }
                }
            }
        }
    }

    void combineOptimizationCoeffs()
    {
        // combine corner coeffs
        for (int i = 0; i < laserCloudCornerLastDSNum; ++i)
        {
            if (laserCloudOriCornerFlag[i] == true)
            {
                laserCloudOri->push_back(laserCloudOriCornerVec[i]);
                coeffSel->push_back(coeffSelCornerVec[i]);
            }
        }
        // combine surf coeffs
        for (int i = 0; i < laserCloudSurfLastDSNum; ++i)
        {
            if (laserCloudOriSurfFlag[i] == true)
            {
                laserCloudOri->push_back(laserCloudOriSurfVec[i]);
                coeffSel->push_back(coeffSelSurfVec[i]);
            }
        }
        // reset flag for next iteration
        std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
        std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
    }

    bool LMOptimization(int iterCount)
    {
        // This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
        // lidar <- camera      ---     camera <- lidar
        // x = z                ---     x = y
        // y = x                ---     y = z
        // z = y                ---     z = x
        // roll = yaw           ---     roll = pitch
        // pitch = roll         ---     pitch = yaw
        // yaw = pitch          ---     yaw = roll

        // lidar -> camera
        float srx = sin(transformTobeMapped[1]);
        float crx = cos(transformTobeMapped[1]);
        float sry = sin(transformTobeMapped[2]);
        float cry = cos(transformTobeMapped[2]);
        float srz = sin(transformTobeMapped[0]);
        float crz = cos(transformTobeMapped[0]);

        int laserCloudSelNum = laserCloudOri->size();
        if (laserCloudSelNum < 50)
        {
            return false;
        }

        cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
        cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
        cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
        cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
        cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
        cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
        cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));

        PointType pointOri, coeff;

        for (int i = 0; i < laserCloudSelNum; i++)
        {
            // lidar -> camera
            pointOri.x = laserCloudOri->points[i].y;
            pointOri.y = laserCloudOri->points[i].z;
            pointOri.z = laserCloudOri->points[i].x;
            // lidar -> camera
            coeff.x = coeffSel->points[i].y;
            coeff.y = coeffSel->points[i].z;
            coeff.z = coeffSel->points[i].x;
            coeff.intensity = coeffSel->points[i].intensity;
            // in camera
            float arx = (crx * sry * srz * pointOri.x + crx * crz * sry * pointOri.y - srx * sry * pointOri.z) * coeff.x + (-srx * srz * pointOri.x - crz * srx * pointOri.y - crx * pointOri.z) * coeff.y + (crx * cry * srz * pointOri.x + crx * cry * crz * pointOri.y - cry * srx * pointOri.z) * coeff.z;

            float ary = ((cry * srx * srz - crz * sry) * pointOri.x + (sry * srz + cry * crz * srx) * pointOri.y + crx * cry * pointOri.z) * coeff.x + ((-cry * crz - srx * sry * srz) * pointOri.x + (cry * srz - crz * srx * sry) * pointOri.y - crx * sry * pointOri.z) * coeff.z;

            float arz = ((crz * srx * sry - cry * srz) * pointOri.x + (-cry * crz - srx * sry * srz) * pointOri.y) * coeff.x + (crx * crz * pointOri.x - crx * srz * pointOri.y) * coeff.y + ((sry * srz + cry * crz * srx) * pointOri.x + (crz * sry - cry * srx * srz) * pointOri.y) * coeff.z;
            // lidar -> camera
            matA.at<float>(i, 0) = arz;
            matA.at<float>(i, 1) = arx;
            matA.at<float>(i, 2) = ary;
            matA.at<float>(i, 3) = coeff.z;
            matA.at<float>(i, 4) = coeff.x;
            matA.at<float>(i, 5) = coeff.y;
            matB.at<float>(i, 0) = -coeff.intensity;
        }

        cv::transpose(matA, matAt);
        matAtA = matAt * matA;
        matAtB = matAt * matB;
        cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

        if (iterCount == 0)
        {

            cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
            cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
            cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));

            cv::eigen(matAtA, matE, matV);
            matV.copyTo(matV2);

            isDegenerate = false;
            float eignThre[6] = {100, 100, 100, 100, 100, 100};
            for (int i = 5; i >= 0; i--)
            {
                if (matE.at<float>(0, i) < eignThre[i])
                {
                    for (int j = 0; j < 6; j++)
                    {
                        matV2.at<float>(i, j) = 0;
                    }
                    isDegenerate = true;
                }
                else
                {
                    break;
                }
            }
            matP = matV.inv() * matV2;
        }

        if (isDegenerate)
        {
            cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
            matX.copyTo(matX2);
            matX = matP * matX2;
        }

        transformTobeMapped[0] += matX.at<float>(0, 0);
        transformTobeMapped[1] += matX.at<float>(1, 0);
        transformTobeMapped[2] += matX.at<float>(2, 0);
        transformTobeMapped[3] += matX.at<float>(3, 0);
        transformTobeMapped[4] += matX.at<float>(4, 0);
        transformTobeMapped[5] += matX.at<float>(5, 0);

        float deltaR = sqrt(
            pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
            pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
            pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
        float deltaT = sqrt(
            pow(matX.at<float>(3, 0) * 100, 2) +
            pow(matX.at<float>(4, 0) * 100, 2) +
            pow(matX.at<float>(5, 0) * 100, 2));

        if (deltaR < 0.05 && deltaT < 0.05)
        {
            return true; // converged
        }
        return false; // keep optimizing
    }

    void scan2MapOptimization()
    {
        if (cloudKeyPoses3D->points.empty())
            return;

        if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
        {
            // std::cout << "kdtree input 0.01: " << std::endl;
            kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
            kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
            // std::cout << "kdtree input 0.02: " << std::endl;

            for (int iterCount = 0; iterCount < 30; iterCount++)
            {
                laserCloudOri->clear();
                coeffSel->clear();
                // gc: calculate some coeff and judge whether tho point is valid corner point
                cornerOptimization();
                // gc: calculate some coeff and judge whether tho point is valid surface point
                surfOptimization();

                combineOptimizationCoeffs();

                // gc: the true iteration steps, calculate the transform
                if (LMOptimization(iterCount) == true)
                    break;
            }
            // gc: interpolate the roll and pitch angle using the IMU measurement and Lidar calculation
            transformUpdate();
        }
        else
        {
            RCLCPP_WARN(get_logger(), "Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
        }
    }
    // gc: interpolate the roll and pitch angle using the IMU measurement and Lidar calculation
    void transformUpdate()
    {
        if (cloudInfo.imu_available == true)
        {
            if (std::abs(cloudInfo.imu_pitch_init) < 1.4)
            {
                double imuWeight = 0.01;
                tf2::Quaternion imuQuaternion;
                tf2::Quaternion transformQuaternion;
                double rollMid, pitchMid, yawMid;

                // slerp roll
                transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
                imuQuaternion.setRPY(cloudInfo.imu_roll_init, 0, 0);
                // gc: interpolate between Imu roll measurement and angle from lidar calculation
                tf2::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                transformTobeMapped[0] = rollMid;

                // slerp pitch
                transformQuaternion.setRPY(0, transformTobeMapped[1], 0);
                imuQuaternion.setRPY(0, cloudInfo.imu_pitch_init, 0);
                // gc: interpolate between Imu roll measurement and angle from lidar calculation
                tf2::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                transformTobeMapped[1] = pitchMid;
            }
        }

        transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
        transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
        transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
    }

    float constraintTransformation(float value, float limit)
    {
        if (value < -limit)
            value = -limit;
        if (value > limit)
            value = limit;

        return value;
    }

    bool saveFrame()
    {
        if (cloudKeyPoses3D->points.empty())
            return true;

        Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
        Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
                                                            transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
        Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);
        // gc: judge whther should generate key pose
        if (abs(roll) < surroundingkeyframeAddingAngleThreshold &&
            abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
            abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
            sqrt(x * x + y * y + z * z) < surroundingkeyframeAddingDistThreshold)
            return false;

        return true;
    }

    void addOdomFactor()
    {
        // gc: the first key pose
        if (cloudKeyPoses3D->points.empty())
        {
            noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI * M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
            gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
            initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
        }
        else
        {
            noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
            gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
            gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);
            // gc: add constraint between current pose and previous pose
            gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size() - 1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
            initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
        }
    }

    void saveKeyFramesAndFactor()
    {
        // gc: judge whther should generate key pose
        if (saveFrame() == false)
            return;

        // odom factor
        // gc: add odom factor in the graph

        addOdomFactor();

        // update iSAM
        isam->update(gtSAMgraph, initialEstimate);
        isam->update();

        gtSAMgraph.resize(0);
        initialEstimate.clear();

        // save key poses
        PointType thisPose3D;
        PointTypePose thisPose6D;
        Pose3 latestEstimate;

        isamCurrentEstimate = isam->calculateEstimate();
        latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size() - 1);
        // cout << "****************************************************" << endl;
        // isamCurrentEstimate.print("Current estimate: ");

        // gc:cloudKeyPoses3D can be used to calculate the nearest key frames
        thisPose3D.x = latestEstimate.translation().x();
        thisPose3D.y = latestEstimate.translation().y();
        thisPose3D.z = latestEstimate.translation().z();
        thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as index
        cloudKeyPoses3D->push_back(thisPose3D);

        thisPose6D.x = thisPose3D.x;
        thisPose6D.y = thisPose3D.y;
        thisPose6D.z = thisPose3D.z;
        thisPose6D.intensity = thisPose3D.intensity; // this can be used as index
        thisPose6D.roll = latestEstimate.rotation().roll();
        thisPose6D.pitch = latestEstimate.rotation().pitch();
        thisPose6D.yaw = latestEstimate.rotation().yaw();
        thisPose6D.time = timeLaserCloudInfoLast;
        cloudKeyPoses6D->push_back(thisPose6D);

        // change-3
        /*added    gc*/
        mtxWin.lock();
        // std::cout <<"in saveKeyFramesAndFactor(): the size of cloudKeyPoses3D is: " << cloudKeyPoses3D->points.size() << std::endl;

        win_cloudKeyPoses3D.push_back(thisPose3D);
        win_cloudKeyPoses6D.push_back(thisPose6D);
        if (win_cloudKeyPoses3D.size() > winSize)
        {
            win_cloudKeyPoses3D.erase(win_cloudKeyPoses3D.begin());
            win_cloudKeyPoses6D.erase(win_cloudKeyPoses6D.begin());
        }

        /*added    gc*/

        // cout << "****************************************************" << endl;
        // cout << "Pose covariance:" << endl;
        // cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl;
        poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size() - 1);

        // save updated transform
        transformTobeMapped[0] = latestEstimate.rotation().roll();
        transformTobeMapped[1] = latestEstimate.rotation().pitch();
        transformTobeMapped[2] = latestEstimate.rotation().yaw();
        transformTobeMapped[3] = latestEstimate.translation().x();
        transformTobeMapped[4] = latestEstimate.translation().y();
        transformTobeMapped[5] = latestEstimate.translation().z();

        // save all the received edge and surf points
        pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
        // gc:
        pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
        pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);

        // save key frame cloud
        cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
        surfCloudKeyFrames.push_back(thisSurfKeyFrame);

        // change-4
        /*added    gc*/

        win_cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
        win_surfCloudKeyFrames.push_back(thisSurfKeyFrame);
        if (win_cornerCloudKeyFrames.size() > winSize)
        {
            win_cornerCloudKeyFrames.erase(win_cornerCloudKeyFrames.begin());
            win_surfCloudKeyFrames.erase(win_surfCloudKeyFrames.begin());
        }

        mtxWin.unlock();
        /*added    gc*/
        pcl::PointCloud<PointType>::Ptr thisIntensityKeyFrame(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr laserCloudIntensityLast_local(new pcl::PointCloud<PointType>());
        pcl::fromROSMsg(cloudInfo.cloud_intensity, *laserCloudIntensityLast_local);
        pcl::copyPointCloud(*laserCloudIntensityLast_local, *thisIntensityKeyFrame);
        intensityCloudKeyFrames.push_back(thisIntensityKeyFrame);

        // save path for visualization
        updatePath(thisPose6D);
    }

    void updatePath(const PointTypePose &pose_in)
    {
        geometry_msgs::msg::PoseStamped pose_stamped;
        pose_stamped.header.stamp = rclcpp::Time(pose_in.time * 1e9);
        pose_stamped.header.frame_id = "odom";
        pose_stamped.pose.position.x = pose_in.x;
        pose_stamped.pose.position.y = pose_in.y;
        pose_stamped.pose.position.z = pose_in.z;
        tf2::Quaternion q;
        q.setRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
        pose_stamped.pose.orientation.x = q.x();
        pose_stamped.pose.orientation.y = q.y();
        pose_stamped.pose.orientation.z = q.z();
        pose_stamped.pose.orientation.w = q.w();

        globalPath.poses.push_back(pose_stamped);
    }

    void publishOdometry()
    {
        // Publish odometry for ROS
        nav_msgs::msg::Odometry laserOdometryROS;
        laserOdometryROS.header.stamp = timeLaserInfoStamp;
        laserOdometryROS.header.frame_id = "odom";
        laserOdometryROS.child_frame_id = "odom_mapping";
        laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
        laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
        laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
        tf2::Quaternion q;
        q.setRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
        laserOdometryROS.pose.pose.orientation = tf2::toMsg(q);
        // laserOdometryROS.pose.covariance[0] = double(imuPreintegrationResetId); // This field is not in ROS2 msg
        pubOdomAftMappedROS->publish(laserOdometryROS);
    }

    void publishFrames()
    {
        if (cloudKeyPoses3D->points.empty())
            return;
        // publish key poses
        publishCloud(pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, "odom");
        // Publish surrounding key frames
        publishCloud(pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, "odom");
        // publish registered key frame
        // gc: feature points
        if (pubRecentKeyFrame->get_subscription_count() > 0)
        {
            pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
            PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
            *cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);
            *cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
            publishCloud(pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, "odom");
        }
        // added *****************by gc
        if (pubLaserCloudInWorld->get_subscription_count() > 0)
        {
            pcl::PointCloud<PointType>::Ptr cloudInBase(new pcl::PointCloud<PointType>());
            pcl::PointCloud<PointType>::Ptr cloudOutInWorld(new pcl::PointCloud<PointType>());
            PointTypePose thisPose6DInOdom = trans2PointTypePose(transformTobeMapped);
            Eigen::Affine3f T_thisPose6DInOdom = pclPointToAffine3f(thisPose6DInOdom);
            mtxtranformOdomToWorld.lock();
            PointTypePose pose_Odom_Map = trans2PointTypePose(tranformOdomToWorld);
            mtxtranformOdomToWorld.unlock();
            Eigen::Affine3f T_pose_Odom_Map = pclPointToAffine3f(pose_Odom_Map);

            Eigen::Affine3f T_poseInMap = T_pose_Odom_Map * T_thisPose6DInOdom;
            *cloudInBase += *laserCloudCornerLastDS;
            *cloudInBase += *laserCloudSurfLastDS;
            pcl::transformPointCloud(*cloudInBase, *cloudOutInWorld, T_poseInMap.matrix());
            publishCloud(pubLaserCloudInWorld, cloudOutInWorld, timeLaserInfoStamp, "map");
        }

        // added *********************by gc
        //  publish registered high-res raw cloud
        // gc: whole point_cloud of the scan
        if (pubCloudRegisteredRaw->get_subscription_count() > 0)
        {
            pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
            pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
            PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
            *cloudOut = *transformPointCloud(cloudOut, &thisPose6D);
            publishCloud(pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, "odom");
        }
        // publish path
        if (pubPath->get_subscription_count() > 0)
        {
            globalPath.header.stamp = timeLaserInfoStamp;
            globalPath.header.frame_id = "odom";
            pubPath->publish(globalPath);
        }
    }

    void loadMaps()
    {
        // 1. 加载用于可视化的完整点云地图 (逻辑不变)
        std::string global_map_path = std::getenv("HOME") + savePCDDirectory + "cloudGlobal.pcd";
        if (pcl::io::loadPCDFile(global_map_path, *cloudGlobalMap) == -1)
        {
            RCLCPP_ERROR(get_logger(), "Failed to load global map from: %s", global_map_path.c_str());
        }
        else
        {
            RCLCPP_INFO(get_logger(), "Loaded global map with %ld points.", cloudGlobalMap->points.size());
            downSizeFilterICP.setInputCloud(cloudGlobalMap);
            downSizeFilterICP.filter(*cloudGlobalMapDS);
            RCLCPP_INFO(get_logger(), "Downsampled global map to %ld points for visualization.", cloudGlobalMapDS->points.size());
        }

        // 2. 加载用于定位的反光柱特征地图
        pillarMap.reset(new pcl::PointCloud<PointType>());
        kdtreePillarMap.reset(new pcl::KdTreeFLANN<PointType>());
        std::string pillar_map_path = std::getenv("HOME") + savePCDDirectory + "PillarMap.pcd";
        if (pcl::io::loadPCDFile(pillar_map_path, *pillarMap) == -1)
        {
            RCLCPP_ERROR(get_logger(), "Failed to load pillar map from: %s", pillar_map_path.c_str());
            RCLCPP_WARN(get_logger(), "Pillar-based localization will not be available.");
        }
        else
        {
            if (pillarMap->points.empty())
            {
                RCLCPP_WARN(get_logger(), "Pillar map is loaded but empty!");
            }
            else
            {
                kdtreePillarMap->setInputCloud(pillarMap);
                RCLCPP_INFO(get_logger(), "Successfully loaded %ld pillars into the feature map.", pillarMap->points.size());
            }
        }
    }

    pcl::PointCloud<PointType>::Ptr extractPillarsFromScan(const pcl::PointCloud<PointType>::Ptr &intensityCloud)
    {
        pcl::PointCloud<PointType>::Ptr pillar_centers(new pcl::PointCloud<PointType>());

        if (intensityCloud->size() < 10)
        {
            return pillar_centers;
        }

        // >>> 关键步骤 1: 高度预过滤 <<<
        // 在聚类前，先用高度滤除大部分来自隧道顶部或地面的无效反光点
        pcl::PointCloud<PointType>::Ptr cloud_filtered_height(new pcl::PointCloud<PointType>());
        pcl::PassThrough<PointType> pass;
        pass.setInputCloud(intensityCloud);
        pass.setFilterFieldName("z"); // 假设雷达坐标系的 Z 轴是向上的
        // !!注意!!: 这个范围需要根据您的雷达安装高度和反光柱实际高度来精确调整
        // 例如，如果雷达在1.5米高，反光柱在0.5米到2.5米之间，可以设置为 (0.0, 3.0)
        pass.setFilterLimits(-1.0, 0.5);
        pass.filter(*cloud_filtered_height);

        // 如果过滤后点太少，直接返回
        if (cloud_filtered_height->size() < 10)
        {
            // 也可以在这里加一个日志，方便调试
            // RCLCPP_WARN(get_logger(), "Not enough points remaining after height filter.");
            return pillar_centers;
        }

        // >>> 关键步骤 2: 在过滤后的干净点云上进行聚类 <<<
        pcl::search::KdTree<PointType>::Ptr tree(new pcl::search::KdTree<PointType>);
        tree->setInputCloud(cloud_filtered_height); // 使用过滤后的点云

        std::vector<pcl::PointIndices> cluster_indices;
        pcl::EuclideanClusterExtraction<PointType> ec;
        ec.setClusterTolerance(0.5); // 在干净数据上可以适当减小此值
        ec.setMinClusterSize(4);     // 尝试减小此值, e.g., 3
        ec.setMaxClusterSize(150);   // 尝试增大此值, e.g., 150 or 200
        ec.setSearchMethod(tree);
        ec.setInputCloud(cloud_filtered_height); // 使用过滤后的点云
        ec.extract(cluster_indices);

        // >>> 关键步骤 3: 可视化聚类结果 (您的代码) <<<
        // 只有当有人订阅这个话题时才执行，避免不必要的计算
        if (pubDebugClusters_->get_subscription_count() > 0)
        {
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
            std::vector<Eigen::Vector3i> colors = {{255, 0, 0}, {0, 255, 0}, {0, 0, 255}, {255, 255, 0}, {0, 255, 255}, {255, 0, 255}};
            int color_index = 0;

            for (const auto &cluster : cluster_indices)
            {
                Eigen::Vector3i color = colors[color_index % colors.size()];
                color_index++;

                for (const auto &point_index : cluster.indices)
                {
                    // 注意：源点来自过滤后的点云
                    PointType original_point = cloud_filtered_height->points[point_index];
                    pcl::PointXYZRGB colored_point;
                    colored_point.x = original_point.x;
                    colored_point.y = original_point.y;
                    colored_point.z = original_point.z;
                    colored_point.r = color[0];
                    colored_point.g = color[1];
                    colored_point.b = color[2];
                    colored_cloud->push_back(colored_point);
                }
            }
            sensor_msgs::msg::PointCloud2 debug_msg;
            pcl::toROSMsg(*colored_cloud, debug_msg);
            // 确保使用正确的header，它包含了坐标系ID和时间戳
            // 假设原始点云的 header 是正确的（例如来自 cloudInfo）
            if (cloudInfoForInitialization)
            { // 确保 cloudInfo 有效
                debug_msg.header = cloudInfoForInitialization->header;
            }
            else
            {
                // 如果在重定位阶段，使用最新的 cloudInfo header
                debug_msg.header = cloudInfo.header;
            }
            pubDebugClusters_->publish(debug_msg);
        }

        // >>> 关键步骤 4: 计算最终的质心 <<<
        for (const auto &cluster : cluster_indices)
        {
            Eigen::Vector4f centroid;
            // 注意：在过滤后的点云上计算质心
            pcl::compute3DCentroid(*cloud_filtered_height, cluster.indices, centroid);

            PointType center_point;
            center_point.x = centroid[0];
            center_point.y = centroid[1];
            center_point.z = centroid[2];
            pillar_centers->push_back(center_point);
        }

        RCLCPP_INFO(get_logger(), "Extracted %ld pillar(s) from current scan.", pillar_centers->points.size());
        return pillar_centers;
    }

    // 基于反光柱匹配来计算位姿的函数
    bool pillarBasedPoseEstimation(const pcl::PointCloud<PointType>::Ptr &live_pillars,
                                   const Eigen::Affine3f &initial_guess,
                                   Eigen::Affine3f &result_pose)
    {
        if (pillarMap->points.empty())
        {
            RCLCPP_WARN_THROTTLE(get_logger(), *this->get_clock(), 5000, "Pillar map is not available, cannot perform feature-based localization.");
            return false;
        }

        if (live_pillars->points.size() < 3)
        {
            RCLCPP_WARN(get_logger(), "Not enough live pillars detected (%ld) to estimate pose. Need at least 3.", live_pillars->points.size());
            return false;
        }

        // ====================== 核心逻辑：动态搜索半径 ======================
        float search_radius;
        if (initializedFlag == Initializing)
        {
            search_radius = 4.0f; // 初始化时，使用4米的搜索半径
            RCLCPP_INFO(get_logger(), "[Pose Estimation] Initializing mode: Using WIDE search radius (%.1f m)", search_radius);
        }
        else
        {
            search_radius = 2.5f; // 重定位时，使用2.5米的严格半径
            RCLCPP_INFO(get_logger(), "[Pose Estimation] Relocalizing mode: Using STRICT search radius (%.1f m)", search_radius);
        }
        // =================================================================

        // --- 数据关联部分 ---
        pcl::PointCloud<PointType>::Ptr source_points(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr target_points(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr transformed_live_pillars(new pcl::PointCloud<PointType>());
        pcl::transformPointCloud(*live_pillars, *transformed_live_pillars, initial_guess);

        // --- DEBUGGING 日志 ---
        RCLCPP_INFO(get_logger(), "--------------------- DEBUGGING CORRESPONDENCES ---------------------");
        float initial_roll, initial_pitch, initial_yaw;
        pcl::getEulerAngles(initial_guess, initial_roll, initial_pitch, initial_yaw);
        RCLCPP_INFO(get_logger(), "Initial Guess (X, Y, Yaw): %f, %f, %f deg", initial_guess.translation().x(), initial_guess.translation().y(), pcl::rad2deg(initial_yaw));

        for (size_t i = 0; i < transformed_live_pillars->points.size(); ++i)
        {
            std::vector<int> pointSearchInd;
            std::vector<float> pointSearchSqDis;
            kdtreePillarMap->nearestKSearch(transformed_live_pillars->points[i], 1, pointSearchInd, pointSearchSqDis);

            PointType live_p_orig = live_pillars->points[i];
            PointType live_p_trans = transformed_live_pillars->points[i];
            PointType map_p = pillarMap->points[pointSearchInd[0]];
            float dist = sqrt(pointSearchSqDis[0]);

            RCLCPP_INFO(get_logger(),
                        "Live Pillar #%ld -> Transformed to (%.2f, %.2f). Found Map Pillar at (%.2f, %.2f). Distance: %.2f m",
                        i, live_p_trans.x, live_p_trans.y, map_p.x, map_p.y, dist);

            if (dist < search_radius)
            {
                source_points->push_back(live_p_orig);
                target_points->push_back(map_p);
            }
        }
        RCLCPP_INFO(get_logger(), "-----------------------------------------------------------------");

        if (source_points->points.size() < 3)
        {
            RCLCPP_WARN(get_logger(), "Not enough valid pillar correspondences found (%ld). Need at least 3.", source_points->points.size());
            return false;
        }

        RCLCPP_INFO(get_logger(), "Found %ld pillar correspondences for pose estimation.", source_points->points.size());

        // =================== 使用SVD进行位姿估计 ===================
        pcl::registration::TransformationEstimationSVD<PointType, PointType> svd_estimator;
        Eigen::Matrix4f transformation_matrix;
        svd_estimator.estimateRigidTransformation(*source_points, *target_points, transformation_matrix);
        result_pose = Eigen::Affine3f(transformation_matrix);

        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(result_pose, x, y, z, roll, pitch, yaw);
        RCLCPP_INFO(get_logger(), "SVD estimated pose: x=%.2f, y=%.2f, yaw=%.2f deg", x, y, pcl::rad2deg(yaw));

        return true;
    }

    void globalLocalizeTimerCallback()
    {
        if (initializedFlag == NonInitialized)
        {
            // 等待RViz的初始位姿，由 initialpose_callback 触发
        }
        else if (initializedFlag == Initializing)
        {
            // 调用新的初始化函数
            RCLCPP_INFO(get_logger(), "Attempting pillar-based initialization...");
            PillarBasedInitialize();
        }
        else // Initialized
        {
            // 调用新的重定位函数
            PillarBasedRelocalize();
        }
    }

    void PillarBasedInitialize()
    {
        std::lock_guard<std::mutex> lock(mtx_general);
        if (!cloudInfoForInitialization)
        {
            RCLCPP_WARN_THROTTLE(get_logger(), *this->get_clock(), 2000, "Waiting for first valid cloud info to initialize...");
            return;
        }

        pcl::PointCloud<PointType>::Ptr intensity_cloud_init(new pcl::PointCloud<PointType>());
        pcl::fromROSMsg(cloudInfoForInitialization->cloud_intensity, *intensity_cloud_init);

        if (intensity_cloud_init->empty())
        {
            RCLCPP_WARN_THROTTLE(get_logger(), *this->get_clock(), 2000, "Waiting for first scan with intensity points to initialize...");
            return;
        }

        // 1. 实时特征提取
        pcl::PointCloud<PointType>::Ptr live_pillars = extractPillarsFromScan(intensity_cloud_init);

        // 2. 获取初始猜测
        Eigen::Affine3f initial_guess = pclPointToAffine3f(trans2PointTypePose(transformInTheWorld));

        // 3. 位姿估计
        Eigen::Affine3f precise_pose;
        if (!pillarBasedPoseEstimation(live_pillars, initial_guess, precise_pose))
        {
            initializedFlag = Initializing;
            RCLCPP_ERROR(get_logger(), "Pillar-based initialization failed.");
            return;
        }

        RCLCPP_INFO(get_logger(), "Pillar-based initialization successful!");
        PointTypePose thisPose6DInOdom = trans2PointTypePose(transformTobeMapped);
        Eigen::Affine3f T_thisPose6DInOdom = pclPointToAffine3f(thisPose6DInOdom);
        Eigen::Affine3f T_OdomToMap = precise_pose * T_thisPose6DInOdom.inverse();

        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(T_OdomToMap, x, y, z, roll, pitch, yaw);

        mtxtranformOdomToWorld.lock();
        tranformOdomToWorld[0] = roll;
        tranformOdomToWorld[1] = pitch;
        tranformOdomToWorld[2] = yaw;
        tranformOdomToWorld[3] = x;
        tranformOdomToWorld[4] = y;
        tranformOdomToWorld[5] = z;
        mtxtranformOdomToWorld.unlock();

        initializedFlag = Initialized;
        std::cout << "Initializing Succeed" << std::endl;
        geometry_msgs::msg::PoseStamped pose_odomTo_map;
        tf2::Quaternion q_odomTo_map;
        q_odomTo_map.setRPY(roll, pitch, yaw);

        pose_odomTo_map.header.stamp = timeLaserInfoStamp;
        pose_odomTo_map.header.frame_id = "map";
        pose_odomTo_map.pose.position.x = x;
        pose_odomTo_map.pose.position.y = y;
        pose_odomTo_map.pose.position.z = z;
        pose_odomTo_map.pose.orientation.x = q_odomTo_map.x();
        pose_odomTo_map.pose.orientation.y = q_odomTo_map.y();
        pose_odomTo_map.pose.orientation.z = q_odomTo_map.z();
        pose_odomTo_map.pose.orientation.w = q_odomTo_map.w();
        pubOdomToMapPose->publish(pose_odomTo_map);
    }

    void PillarBasedRelocalize()
    {
        if (cloudKeyPoses3D->points.empty())
            return;

        pcl::PointCloud<PointType>::Ptr latest_intensity_cloud;
        PointTypePose latest_pose_6d;

        // 1. 获取最新关键帧数据 (逻辑不变)
        {
            std::lock_guard<std::mutex> lock(mtx);
            if (intensityCloudKeyFrames.empty() || cloudKeyPoses6D->empty())
            {
                RCLCPP_WARN(get_logger(), "Not enough keyframes to perform relocalization.");
                return;
            }
            latest_intensity_cloud = intensityCloudKeyFrames.back();
            latest_pose_6d = cloudKeyPoses6D->back();
        }

        // 2. 实时特征提取 (逻辑不变)
        pcl::PointCloud<PointType>::Ptr live_pillars = extractPillarsFromScan(latest_intensity_cloud);

        // 3. 获取初始猜测 (逻辑不变)
        Eigen::Affine3f T_thisPose6DInOdom = pclPointToAffine3f(latest_pose_6d);
        Eigen::Affine3f T_OdomToMap_current;
        {
            std::lock_guard<std::mutex> lock(mtxtranformOdomToWorld);
            T_OdomToMap_current = pclPointToAffine3f(trans2PointTypePose(tranformOdomToWorld));
        }
        Eigen::Affine3f initial_guess = T_OdomToMap_current * T_thisPose6DInOdom;

        // 4. 位姿估计 (逻辑不变)
        Eigen::Affine3f precise_pose_in_map; // 这是在map坐标系下的精确位姿
        if (!pillarBasedPoseEstimation(live_pillars, initial_guess, precise_pose_in_map))
        {
            RCLCPP_WARN(get_logger(), "Pillar-based relocalization failed for this frame.");
            return;
        }

        // ====================== 核心修改：重置LIO后端 ======================
        RCLCPP_INFO(get_logger(), "\033[1;32mRelocalization successful! Resetting LIO backend.\033[0m");

        // 加一个大锁，确保在重置时，没有其他线程在操作后端数据
        std::lock_guard<std::mutex> lock(mtx);

        // 5. 计算出这个精确位姿在当前的 odom 坐标系下应该是什么样子
        // T_map_odom * T_odom_precise = T_map_precise  => T_odom_precise = T_map_odom.inverse() * T_map_precise
        Eigen::Affine3f precise_pose_in_odom = T_OdomToMap_current.inverse() * precise_pose_in_map;

        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(precise_pose_in_odom, x, y, z, roll, pitch, yaw);

        // 6. 重置整个后端优化器（ISAM2, 因子图, 关键帧等）
        resetLIO();

        // 7. 将这个精确的位姿作为新的“第一帧”加入到GTSAM中
        transformTobeMapped[0] = roll;
        transformTobeMapped[1] = pitch;
        transformTobeMapped[2] = yaw;
        transformTobeMapped[3] = x;
        transformTobeMapped[4] = y;
        transformTobeMapped[5] = z;

        // 在因子图中加入一个带有强约束的先验因子，告诉优化器：“我确定现在的位置就在这里”
        noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances(
            (Vector(6) << 1e-2, 1e-2, M_PI * M_PI, 1e8, 1e8, 1e8).finished());
        gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
        initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));

        // 8. 强制GTSAM更新，接受这个新的起点
        isam->update(gtSAMgraph, initialEstimate);
        gtSAMgraph.resize(0);
        initialEstimate.clear();

        // 9. 最后，更新 odom -> map 的变换
        Eigen::Affine3f T_OdomToMap_new = precise_pose_in_map * pclPointToAffine3f(trans2PointTypePose(transformTobeMapped)).inverse();
        pcl::getTranslationAndEulerAngles(T_OdomToMap_new, x, y, z, roll, pitch, yaw);
        {
            std::lock_guard<std::mutex> lock2(mtxtranformOdomToWorld);
            tranformOdomToWorld[0] = roll;
            tranformOdomToWorld[1] = pitch;
            tranformOdomToWorld[2] = yaw;
            tranformOdomToWorld[3] = x;
            tranformOdomToWorld[4] = y;
            tranformOdomToWorld[5] = z;
        }
    }

    void keyFramesLoad()
    {
    }

    void initialpose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose_msg)
    {
        // first calculate global pose
        // x-y-z
        if (initializedFlag == Initialized)
            return;

        float x = pose_msg->pose.pose.position.x;
        float y = pose_msg->pose.pose.position.y;
        float z = pose_msg->pose.pose.position.z;

        // roll-pitch-yaw
        tf2::Quaternion q_global;
        double roll_global;
        double pitch_global;
        double yaw_global;

        q_global.setX(pose_msg->pose.pose.orientation.x);
        q_global.setY(pose_msg->pose.pose.orientation.y);
        q_global.setZ(pose_msg->pose.pose.orientation.z);
        q_global.setW(pose_msg->pose.pose.orientation.w);

        tf2::Matrix3x3(q_global).getRPY(roll_global, pitch_global, yaw_global);
        // global transformation
        transformInTheWorld[0] = roll_global;
        transformInTheWorld[1] = pitch_global;
        transformInTheWorld[2] = yaw_global;
        transformInTheWorld[3] = x;
        transformInTheWorld[4] = y;
        transformInTheWorld[5] = z;
        PointTypePose thisPose6DInWorld = trans2PointTypePose(transformInTheWorld);
        Eigen::Affine3f T_thisPose6DInWorld = pclPointToAffine3f(thisPose6DInWorld);
        // Odom transformation
        PointTypePose thisPose6DInOdom = trans2PointTypePose(transformTobeMapped);
        Eigen::Affine3f T_thisPose6DInOdom = pclPointToAffine3f(thisPose6DInOdom);
        // transformation: Odom to Map
        Eigen::Affine3f T_OdomToMap = T_thisPose6DInWorld * T_thisPose6DInOdom.inverse();
        float delta_x, delta_y, delta_z, delta_roll, delta_pitch, delta_yaw;
        pcl::getTranslationAndEulerAngles(T_OdomToMap, delta_x, delta_y, delta_z, delta_roll, delta_pitch, delta_yaw);

        mtxtranformOdomToWorld.lock();
        // keep for co-operate the initializing and lio, not useful for the present
        tranformOdomToWorld[0] = delta_roll;
        tranformOdomToWorld[1] = delta_pitch;
        tranformOdomToWorld[2] = delta_yaw;
        tranformOdomToWorld[3] = delta_x;
        tranformOdomToWorld[4] = delta_y;
        tranformOdomToWorld[5] = delta_z;

        mtxtranformOdomToWorld.unlock();
        initializedFlag = Initializing;

        // globalLocalizeInitialized = false;
    }

    /*************added by gc******************/
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    rclcpp::NodeOptions options;
    options.use_intra_process_comms(true);
    rclcpp::executors::MultiThreadedExecutor exec;

    auto MO = std::make_shared<mapOptimization>(options);
    exec.add_node(MO);

    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "\033[1;32m----> Map Optimization Started.\033[0m");

    exec.spin();

    rclcpp::shutdown();

    return 0;
}